#include "cloud_sea/cloud_sea.hpp"
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <memory>
#include <string>
#include <limits>
#include <algorithm>

using namespace std::chrono_literals;

namespace cloud_sea {

CloudSea::CloudSea(const rclcpp::NodeOptions & options)
    : Node("cloud_sea", options) 
{
    // 声明参数
    pointcloud_topic_ = this->declare_parameter("pointcloud_topic", "/camera/point_cloud");
    
    // 初始化边界值
    min_x_ = std::numeric_limits<double>::max();
    max_x_ = std::numeric_limits<double>::lowest();
    min_y_ = std::numeric_limits<double>::max();
    max_y_ = std::numeric_limits<double>::lowest();
    
    // 创建订阅者
    pointcloud_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
        pointcloud_topic_, 
        rclcpp::SensorDataQoS(),
        std::bind(&CloudSea::pointCloudCallback, this, std::placeholders::_1));
    
    RCLCPP_INFO(this->get_logger(), "CloudSea node started. Subscribing to topic: %s", 
               pointcloud_topic_.c_str());
}

void CloudSea::pointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) 
{
    // 重置边界值
    min_x_ = std::numeric_limits<double>::max();
    max_x_ = std::numeric_limits<double>::lowest();
    min_y_ = std::numeric_limits<double>::max();
    max_y_ = std::numeric_limits<double>::lowest();
    
    // 转换为PCL点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::fromROSMsg(*msg, *cloud);
    
    RCLCPP_INFO(this->get_logger(), "Received point cloud with %zu points", cloud->size());
    
    if (cloud->empty()) {
        RCLCPP_WARN(this->get_logger(), "Point cloud is empty");
        return;
    }
    
    // 遍历所有点，计算XY边界值
    for (const auto& point : cloud->points) {
        // 跳过无效点
        if (!std::isfinite(point.x) || !std::isfinite(point.y) || !std::isfinite(point.z)) {
            continue;
        }
        
        // 更新X边界
        min_x_ = std::min(min_x_, static_cast<double>(point.x));
        max_x_ = std::max(max_x_, static_cast<double>(point.x));
        
        // 更新Y边界
        min_y_ = std::min(min_y_, static_cast<double>(point.y));
        max_y_ = std::max(max_y_, static_cast<double>(point.y));
    }
    
    // 输出结果
    RCLCPP_INFO(this->get_logger(), "Point Cloud XY Boundaries:");
    RCLCPP_INFO(this->get_logger(), "X Range: [%.6f, %.6f]", min_x_, max_x_);
    RCLCPP_INFO(this->get_logger(), "Y Range: [%.6f, %.6f]", min_y_, max_y_);
    RCLCPP_INFO(this->get_logger(), "X Span: %.6f, Y Span: %.6f", 
               max_x_ - min_x_, max_y_ - min_y_);
}

} // namespace cloud_sea


int main(int argc, char * argv[]) {
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<cloud_sea::CloudSea>());
    rclcpp::shutdown();
    return 0;
}